import os
import platform
import serial
import sys
import time

sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")))
from tools import get_yaml
from log_info import logger


class Arm:
    def __init__(self, get_ser=None):
        self.ser = None
        self.cfg = get_yaml("arm/base/arm.yml")
        self.pwm = {"pwm1": "01", "pwm2": "02", "pwm3": "03", "pwm4": "04", "pwm5": "05", "pwm6": "07"}
        if get_ser is not None:
            self.get_server(get_ser)

    def get_server(self, car):
        self.ser = car.base_moto.ser
        self.ser.do = self.ser.get_answer

    def do_task(self, task, wait=0.7):
        if self.ser is not None:
            task = self.cfg[task]
            for step in task:
                for pwm in task[step]:
                    port = self.pwm[pwm]
                    speed = hex(task[step][pwm]['speed'])[2:]
                    if pwm != 'pwm2':
                        angle = format(int((task[step][pwm]['angle']) * 2 / 3), "02x")  # 根据舵机的参数进行解算
                    else:
                        angle = format(task[step][pwm]['angle'], "02x")
                    command = f"77 68 09 05 02 {port} {speed} {angle} 0A"
                    if pwm == "pwm5":  # 这里是因为下位机的5号和6号口出错需要同时发指令
                        sub_port = "06"
                        sub_command = f"77 68 09 05 02 {sub_port} {speed} {angle} 0A"
                        sub_command_bytes = bytes.fromhex(sub_command)
                        self.ser.do(sub_command_bytes, 0.5)
                        self.ser.do(sub_command_bytes, 0.5)
                        self.ser.do(sub_command_bytes, 0.5)
                    command_bytes = bytes.fromhex(command)
                    self.ser.do(command_bytes, 0.5)  # 多次发送保证数据的有效执行
                    self.ser.do(command_bytes, 0.5)
                    self.ser.do(command_bytes, 0.5)
                    time.sleep(wait)
        else:
            logger.info("arm还没有获取服务")


class Arm_test(Arm):
    def __init__(self):
        super(Arm, self).__init__()
        if "Windows" in platform.platform():
            self.ser = serial.Serial('COM8', 1000000)
        else:
            self.ser = serial.Serial('/dev/ttyUSB0', 1000000)

        self.ser.do = self.ser.write
        self.cfg = get_yaml("arm/base/arm.yml")
        self.pwm = {"pwm1": "01", "pwm2": "02", "pwm3": "03", "pwm4": "04", "pwm5": "05", "pwm6": "07"}


if __name__ == "__main__":
    my_arm = Arm_test()
    my_arm.do_task("det_pos_right_plus")
